/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for SpeedCtrl interface client library.
 * \author Antti Maula <antti.maula@tkk.fi>
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * \todo Not yet complete. ThreadHandling function
 *       missing. Also, no specification for subscribed data.
 *
 */

#include "SpeedCtrlClient.hpp"
#include "MaCICtrlClient.hpp"

//*****************************************************************************
using namespace MaCI::SpeedCtrl;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CSpeedCtrlClient::CSpeedCtrlClient(gimi::GIMI *aGIMIPtr, 
                                   const int aInterfaceMinor)
  : CSync(3,1),
    CThread(1),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_SPEEDCTRL, aInterfaceMinor),
    iMaxInterval(0),
    iMinInterval(0),
    iIntervalsSet(false),
    iLastCommandTime(0),
    iServerSideTimeout(0),
    iLastCommandType(KCommandUnknown),
    iLastSpeed(0.0),
    iLastAngularSpeed(0.0),
    iLastAcceleration(0.0),
    iMaxSpeeds(),
    iMinSpeeds(),
    iSpeedLimitsSet(false),
    iSpeedLimitsAreKnown(false),
    iSpeedData(),
    iSpeedSeq(-1),
    iSubscribedToSpeedService(false),
    iSpeedCtrlClientCallback(NULL),
    iCallBackUserPtr(NULL)

{
  dPrint(4,"CSpeedCtrlClient %p constructed", this);
}
//*****************************************************************************

CSpeedCtrlClient::~CSpeedCtrlClient()
{
  DoClose();
  dPrint(4,"CSpeedCtrlClient %p destructed", this);
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlClient::DoOpen(void)
{
  
  EMaCIError result = KMaCIError;
  const unsigned int total_timeout = TimeoutValue(3000);
  const ownTime_ms_t begin = ownTime_get_ms();

  // Now, start checking.
  if(!IsThreadRunning()){
    dPrintLCGreen(ODTEST,"Thread is not running. Good");
    
    // Internal check done. Now check remotely whether the service is
    // accepted on the selected remote service.

    if (IsServiceAcceptedOnTarget(total_timeout) == KMaCIOK) {
      //      dPrintLCGreen(ODTEST,"Service IS accepted on target. Good!");
      
      RegisterAcceptedService("MaCI-SpeedCtrlClient");
      EnableSynchronousMessageReply();
      SetActive(true);
      CThread::RunThread();
      
      //dPrintLCGreen(ODTEST,"Now, Get intervals.");
      result = GetIntervalsFromServer(ownTime_get_ms_left(total_timeout, begin));
      if (result == KMaCIOK) //dPrintLCGreen(ODTEST,"OK"); else dPrintLCRed(ODTEST,"FAILED!");

        //dPrintLCGreen(ODTEST,"Get limits.");
      if (result == KMaCIOK) result = GetSpeedLimitsFromServer(ownTime_get_ms_left(total_timeout, begin));

      if (result == KMaCIOK) //dPrintLCGreen(ODTEST,"OK"); else dPrintLCRed(ODTEST,"FAILED!");

        //dPrintLCGreen(ODTEST,"Get timeouttime.");
      if(result == KMaCIOK) result = GetSpeedCtrlServerTimeoutTime(iServerSideTimeout, ownTime_get_ms_left(total_timeout, begin)) ? KMaCIOK : KMaCIError;

      /*if (result == KMaCIOK)// dPrintLCGreen(ODTEST,"OK"); else dPrintLCRed(ODTEST,"FAILED!");
      
        if (result == KMaCIOK) //dPrintLCGreen(ODTEST,"All OK!");*/
    } else {
      dPrintLCGreen(ODTEST,"Current target (%s) for SpeedCtrlClient does not accept SpeedCtrl commands -> Invalid!",
                   iDefaultTarget.c_str());
    }
    
  } else {
    dPrint(ODWARN,"SpeedCtrlClient %p is already Open to '%s'!", 
           iDefaultTarget.c_str());
  }



  // Final check.
  if(result == KMaCIOK){
    //    SetActive(true);
    
    /*
    // FIXME: DON'T VALIDATE - IT BREAKS THE MACICTRLSERVER!!!!!!!!!!!!!
    // Do unconditional call to RevalidateInterfaceAccess();
    MaCICtrl::TAccess currentaccess;
    ValidateClientInterfaceAuthentication(currentaccess);
    */
#warning Authentication validation not properly implemented
  }

  return result;
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlClient::DoClose(void)
{
  EMaCIError result = KMaCIError;

  if(IsThreadRunning()){

    SetActive(false);

    // Call Unregister.
    UnsubscribeToInterface();  //QUICKFIX, Until UnregisterAcceptedService is working properly
    UnregisterAcceptedService();

    DisableSynchronousMessageReply();

    if(iSubscribedToSpeedService){

      StopGettingSpeedEvent();
    } 

    // Now break everything :)
    //    CancelThread();
    WaitThread();


    result = KMaCIOK;
  }else{
    dPrint(3,"SpeedCtrl client already closed!");
    result = KMaCIError;
  }

  return result;
}
//*****************************************************************************

bool CSpeedCtrlClient::SetSpeed(const float aSpeed, 
                                const float aAngularSpeed, 
                                const float aAcceleration)
{
  // Construct command to set speed
  bool sendCommand = false;
  bool result = false;
  Lock();
  //check last command type
  if(iLastCommandType != KCommandSetSpeed){
    sendCommand = true;
  }

  if(!sendCommand){ 
    //If speed are same than previously then check when last command is sent. 
    //If time from last commandis bigger than the minimum time interval, send same speeds again
    if(aSpeed == iLastSpeed && aAngularSpeed == iLastAngularSpeed && aAcceleration == iLastAcceleration){
      int since = (ownTime_get_ms_since(iLastCommandTime));
      if(since > (int)iMaxInterval){
        sendCommand = true;
        result = true;
      }else{
        dPrint(6,"ignoring sending same speeds, because %d ms has elapsed from last command. Sending same speeds every %d ms",ownTime_get_ms_since(iLastCommandTime),iMaxInterval);
        result = true;
      }
    }else{
      int since = (ownTime_get_ms_since(iLastCommandTime));
      bool istrue = since >((int)iMinInterval);

      dPrint(8,"min interval %d ms. Time from last command %d, true?? %d", iMinInterval,since,(int)istrue);

      if(since >(int)iMinInterval){
        sendCommand = true;
        result = true;
      }else{
        dPrint(3,"Too many commands. Accepting commands only every %d ms. Time from last command %d", (int)iMinInterval,ownTime_get_ms_since(iLastCommandTime));
        result = false;
      }
    }
  }
  Unlock();

  //Checking if sending commands too slowly. If yes print warning
  //Just to inform the user...

  if(ownTime_get_ms_since(iLastCommandTime) > ((ownTime_ms_delta_t)iServerSideTimeout)){

    if(iLastCommandType == KCommandSetSpeed && 
       (iLastSpeed > 0.01 || iLastSpeed < -0.01 || iLastAngularSpeed > 0.01 || iLastAngularSpeed < -0.01)){
     

      dPrintLC(1,
               OWNDEBUG_COLOR_ATTR_BRIGHT,OWNDEBUG_COLOR_RED, 
               OWNDEBUG_COLOR_BLACK, 
               "Warning: You are sending set speed commands too slowly. Server side usually stops the robot if no command is arrived within %ld ms(your last SetSpeed-command was sent %ld ms ago)",
               (ownTime_ms_delta_t)iServerSideTimeout, 
               ownTime_get_ms_since(iLastCommandTime));
      
          
     
    }
    
  }

  if(sendCommand){   
    gim::binbag::CBinBag bb;
    CSpeedCtrlData ctrl;
    ctrl.EncodeTo(&bb);
    ctrl.SetCommand(TCommand(KCommandSetSpeed));
    ctrl.SetSpeed(TSpeed(aSpeed, aAngularSpeed, aAcceleration));
    
    // Send the command

    const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                              bb.GetBufferSize());


    Lock();
    if(e != MaCI::KMaCIOK){
      dPrint(3,"Failed to Send SpeedCtrl interface message : '%s'",GetErrorStr(e).c_str());
      return false;
    }
    iLastCommandTime = ownTime_get_ms();
    iLastCommandType = KCommandSetSpeed;
    iLastSpeed = aSpeed;
    iLastAngularSpeed = aAngularSpeed;
    iLastAcceleration = aAcceleration;
    Unlock();
    return (e == KMaCIOK);
  }

  return result;
}
//*****************************************************************************

bool CSpeedCtrlClient::SetStop(void)
{ 
  // Construct command to set speed

  // Construct command to set speed
  bool sendCommand = false;
  bool result = false;
  Lock();
  //check last command type
  if(iLastCommandType != KCommandSetStop){
    sendCommand = true;
  }

  if(!sendCommand){ 
    if(ownTime_get_ms_since(iLastCommandTime) > (int)iMinInterval){
      sendCommand = true;
      result = true;
    }else{
      dPrint(8,"ignoring sending same setStops, because %d ms has elapsed from last setStop-command. Sending same commands every %d ms",ownTime_get_ms_since(iLastCommandTime),iMaxInterval);
      result = true;
    }
  }
  Unlock();
  if(sendCommand){
    gim::binbag::CBinBag bb;
    CSpeedCtrlData ctrl;
    ctrl.EncodeTo(&bb);
    ctrl.SetCommand(TCommand(KCommandSetStop));
    
    // Send the command
    const EMaCIError e = SendInterfaceMessage(bb.GetBuffer(),
                                              bb.GetBufferSize());
    Lock();

    iLastCommandTime = ownTime_get_ms();
    iLastCommandType = KCommandSetStop;
    Unlock();
    return (e == KMaCIOK);
  }
  return result;
}
//*****************************************************************************

bool CSpeedCtrlClient::GetSpeedLimits(TSpeed &aMaxSpeeds, TSpeed &aMinSpeeds)
{
  if(iSpeedLimitsAreKnown){
  
    Lock();

    aMaxSpeeds = iMaxSpeeds;
    aMinSpeeds  = iMinSpeeds;
    
    Unlock();
  
  }

  return iSpeedLimitsAreKnown;

}
//*****************************************************************************

bool CSpeedCtrlClient::StartGettingSpeedEvent(void)
{
  bool result = false;
  if(!iSubscribedToSpeedService && IsThreadRunning()){
   
    EMaCIError res = SubscribeToInterface();
    if(res == KMaCIOK){
      iSubscribedToSpeedService = true;
      result = true;
    }
  

  }
  return result;
}
//*****************************************************************************

bool CSpeedCtrlClient::StopGettingSpeedEvent(void)
{
  bool result = false;
  if(iSubscribedToSpeedService){
    // Attempt to subscribe to service.
    EMaCIError res = UnsubscribeToInterface();
    if(res == KMaCIOK){
      iSubscribedToSpeedService = false;
      result = true;

    }
    /*
    int r = iGIMIPtr->unsubscribeService(iDefaultTarget,
                                         iInterfaceMajor,
                                         iInterfaceMinor,
                                         TimeoutValue(1000)); // Fixed to static+1s
    
    switch(r) {
    case GIMI_OK:
      dPrint(3,"Succesfully Unsubscribed from SpeedCtrlServer (%d:%d) at target '%s' : '%s'",
             iInterfaceMajor,
             iInterfaceMinor,
             iDefaultTarget.c_str(),
             gimi::getGimiErrorString(r).c_str());
      result = true;
      break;
      
    case GIMI_TIMEOUT:
    case GIMI_INVALIDCLIENT:
    case GIMI_INVALIDTYPE:
    case GIMI_NOCONNECTION:
    case GIMI_ERROR:
    default:
      dPrint(2,"Failed to Unsubscribe from SpeedCtrlServer (%d:%d) at target '%s' : '%s'",
             iInterfaceMajor,
             iInterfaceMinor,
             iDefaultTarget.c_str(),
             gimi::getGimiErrorString(r).c_str());
      result = false;
      break;
    }
    
    
    if(result){
      iSubscribedToSpeedService = false;
    }
    */
  }
  return result;
  
}
//*****************************************************************************

bool CSpeedCtrlClient::GetSpeed(float &aSpeed, 
                                float &aAngularSpeed, 
                                float &aAcceleration,
                                MaCI::Common::TTimestamp &aTimestamp,
                                int *aSequence, 
                                unsigned int aTimeout_ms)
{
  bool result = true;
  //Not subscribed to service. First subscribe the 
  

  //If everything is ok, let's wait for data...
  if(iSubscribedToSpeedService && IsThreadRunning()){
    dPrint(15,"Subscribed.. then copying the data");
    Lock();
    if (aSequence == NULL) {
      // No sequence number defined. Just do direct copy of local data - no sync
      dPrint(15,"No seq number defined.  Just do direct copy of local data - no sync");
      if(iSpeedData.GetSpeed()){
        dPrint(15,"Speed data found");
        dPrint(15,"Speed %f, AS %f ,acc %f",
               iSpeedData.GetSpeed()->speed,
               iSpeedData.GetSpeed()->angularSpeed,
               iSpeedData.GetSpeed()->acceleration);
        
        aSpeed = iSpeedData.GetSpeed()->speed;
        aAngularSpeed = iSpeedData.GetSpeed()->angularSpeed;
        aAcceleration= iSpeedData.GetSpeed()->acceleration;
        if(iSpeedData.GetTimestamp()){
          aTimestamp.timestamp_s = iSpeedData.GetTimestamp()->timestamp_s;
          aTimestamp.timestamp_us = iSpeedData.GetTimestamp()->timestamp_us;
        }
        result = true;
      }else{
        dPrint(4,"CSpeedData doesn't contain speed data. Perhaps no data received at all...");
        result = false;
      }
      
    } else {

      if (iSpeedSeq <= *aSequence) {
        dPrint(10,"Waiting for new data");
        int time_left = aTimeout_ms;
        unsigned int time_begin = ownTime_get_ms();
        
        // The available scan is older than requested - must wait for time_left
       
        do {
          Wait(time_left, KEventSpeedData);
          time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
          
        } while(IsThreadRunning() && iSpeedSeq < *aSequence && time_left > 0);
       

        // Now, check whether the waiting resulted in the correct sequence
        if (iSpeedSeq > *aSequence) {
          // Yes :)
          if(iSpeedData.GetSpeed()){
            dPrint(15,"Speed %f, AS %f ,acc %f",iSpeedData.GetSpeed()->speed,
                   iSpeedData.GetSpeed()->angularSpeed,
                   iSpeedData.GetSpeed()->acceleration);
            aSpeed = iSpeedData.GetSpeed()->speed;
            aAngularSpeed = iSpeedData.GetSpeed()->angularSpeed;
            aAcceleration= iSpeedData.GetSpeed()->acceleration;
            if(iSpeedData.GetTimestamp()){
              aTimestamp.timestamp_s = iSpeedData.GetTimestamp()->timestamp_s;
              aTimestamp.timestamp_us = iSpeedData.GetTimestamp()->timestamp_us;
            }
            *aSequence = iSpeedSeq;
            result = true;
          }else{
            result = false;
          }

          
        } else {
          // No :(
          dPrint(3, "Failed to get SpeedCtrlData! (Timeout %dms)",
                 aTimeout_ms);

          result = false;
        }
        
      } else {
        // The available data is newer or matching.
        dPrint(15,"There is already new data");
        if(iSpeedData.GetSpeed()){
          dPrint(15,"It contains speed data");
          dPrint(15,"Speed %f, AS %f ,acc %f",iSpeedData.GetSpeed()->speed,
                 iSpeedData.GetSpeed()->angularSpeed,
                 iSpeedData.GetSpeed()->acceleration);

          aSpeed = iSpeedData.GetSpeed()->speed;
          aAngularSpeed = iSpeedData.GetSpeed()->angularSpeed;;
          aAcceleration= iSpeedData.GetSpeed()->acceleration;;
          if(iSpeedData.GetTimestamp()){
            aTimestamp.timestamp_s = iSpeedData.GetTimestamp()->timestamp_s;
            aTimestamp.timestamp_us = iSpeedData.GetTimestamp()->timestamp_us;
          }
          *aSequence = iSpeedSeq;
          result = true;
        }else{
          dPrint(1,"The available data doesn't contain Speed data.. something is wrong ;)");
          result = false;
        }
        //      dPrint(1,"Got data. *aSeq = %d, iPositionDataSequence = %d", *aSeq, iPositionDataSequence);
       
      }
    }
    
    
    Unlock();
  }else{
    if(!iSubscribedToSpeedService){
      dPrint(3,"SpeedCtrlClient has not subscribed to service. Please call StartGettingSpeedEvent-function first");
    }

    if(!IsThreadRunning()){
      dPrint(3,"Client is not active");
    }

    result = false;
  }


  return result;
}

//*****************************************************************************

bool CSpeedCtrlClient::GetSpeedCtrlServerTimeoutTime(ownTime_ms_t &aServerTimeoutTime,
                                                     unsigned int aTimeout_ms)
{
  // return result,
  bool result = false;
  
    
  // Construct command.
  CSpeedCtrlData cmd;
  CSpeedCtrlData reply;
  SMessageSource scms;
  cmd.CreateInternalBinBag();
  cmd.SetCommand(TCommand(KCommandGetTimeoutTime));
  
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  // Send command.
  SendInterfaceMessage(cmd, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(reply, scms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    // Got reply.
    const TTimeoutTime *to = reply.GetTimeoutTime();
    if (to) {
      aServerTimeoutTime = (ownTime_ms_t)(to->timeoutTime);
       
      result = true;
    } else {
      dPrint(ODWARN,"Received reply without TimeoutTime-struct!");
      
    }
  }

  return result;
 

}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//**** Privates ***************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlClient::GetSpeedLimitsFromServer(const unsigned int &aTimeout_ms)
{
  EMaCIError result = KMaCIError; 
  const unsigned int begin = ownTime_get_ms();
  unsigned int timeout = aTimeout_ms;
  int timeLeft = 0;
  gim::binbag::CBinBag bb;
  CSpeedCtrlData ctrl;
  ctrl.EncodeTo(&bb);
  ctrl.SetCommand(TCommand(KCommandGetSpeedLimits));
  result = SendInterfaceMessage(bb.GetBuffer(),
                                bb.GetBufferSize());
  
  if (result == KMaCIOK) {
    
    timeLeft = timeout - ownTime_get_ms_since(begin);
    Lock();
    while (timeLeft > 0 && !iSpeedLimitsSet) {
      Wait(timeLeft, KReplySpeeds);
      
      //Check for timeout
      timeLeft = timeout - ownTime_get_ms_since(begin);
    }
    Unlock();

    if(iSpeedLimitsSet) {
      
      //Default values, so no real speeds set at the server.
      if (iMaxSpeeds.speed == 0.0f && iMinSpeeds.speed == 0.0f && 
          iMaxSpeeds.angularSpeed == 0.0f && iMinSpeeds.angularSpeed == 0.0f &&
          iMaxSpeeds.acceleration == 0.0f && iMinSpeeds.acceleration == 0.0f) {
        iSpeedLimitsAreKnown = false;
        
      } else {
        iSpeedLimitsAreKnown = true;
        dPrint(8,"Got speedlimits at client max(%f %f %f) min(%f %f %f)", 
               iMaxSpeeds.speed, iMaxSpeeds.angularSpeed, iMaxSpeeds.acceleration,
               iMinSpeeds.speed, iMinSpeeds.angularSpeed, iMinSpeeds.acceleration);

      }
    } else
      {
        // We did not get speed limits within the timeout, so we return KMaCIError
         result = KMaCITimeout;

      }
  } else {
    dPrint(3,"Failed to send SpeedCtrl interface message! Can't open SpeedCtrlClient!");
    result = KMaCIError;

  }
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CSpeedCtrlClient::GetIntervalsFromServer(const unsigned int &aTimeout_ms)
{
  EMaCIError result = KMaCIError;
  const unsigned int begin = ownTime_get_ms();
  unsigned int timeout = aTimeout_ms;
  int timeLeft = 0;
  gim::binbag::CBinBag bb;
  CSpeedCtrlData ctrl;
  ctrl.EncodeTo(&bb);
  ctrl.SetCommand(TCommand(KCommandGetCommandIntervals));
  result = SendInterfaceMessage(bb.GetBuffer(), bb.GetBufferSize());
  if(result == KMaCIOK){
    timeLeft = timeout - ownTime_get_ms_since(begin);
    Lock();
    while (timeLeft > 0 && !iIntervalsSet) {

      Wait(timeLeft,KReplyIntervals);
      //Check for timeout
      timeLeft = timeout - ownTime_get_ms_since(begin);

    }
    Unlock();

    
    if (!iIntervalsSet) {
      dPrint(3,"Failed to get command sending intervals values from server! Can't open speedCtrlClient!");
      result = KMaCIError;

    } else {
      dPrint(8,"Got intervals (max: %u, min: %u)", iMaxInterval,iMinInterval);

    }
  } else {
    dPrint(ODSTATUS, "Failed to send SpeedCtrl interface message! Unable to open the SpeedCtrlClient!");
    result = KMaCIError;

  }
  
  return result;
}
//*****************************************************************************

int CSpeedCtrlClient::ThreadFunction(const int aThreadNumber)
{
 
  int result = 0;
  EMaCIError e;
  gimi::GIMIMessage gmsg;
  
  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(4, "GIMIMessage handler started (threadID %p)", pthread_self());
  
  // Run as long as the thread should be active
  while(IsActive()) {
    
    // Call the ReceiveMessage wrapper with 1s timeout

    e = ReceiveInterfaceMessage(gmsg, 24*60*60*1000);

    if (e == KMaCIOK) {
      
      ProcessMessage(gmsg);
      
    }else if(e != KMaCITimeout){
      if(IsActive()){
        // Other error, try to recover by sleeping one second.
        dPrint(3,"ProcessMessage returned error %u and client still active. Trying to recover by sleeping 1000ms ",
               e);
        ownSleep_ms(1000);
      }else{
        //
        dPrint(3,"ProcessMessage returned error %u. Normal situtation because client is already closed",
               e);
      }
      
      //      dPrint(1,"Timeout...");
    }
  }
  
  dPrint(4, "GIMIMessage handler stopped (threadID %p)", pthread_self());

  return result;
}
//*****************************************************************************

int CSpeedCtrlClient::ProcessMessage(const gimi::GIMIMessage &msg)
{

  int result = 0;
  gim::binbag::CBinBag bb;
  CSpeedCtrlData d;
  bool r;


  r = bb.Decode(msg.getData(), msg.getDatalength());

  if (r) {
    // Succesfully decoded as BinBag.
    r = d.DecodeFrom(&bb);
    if (r) {
      // Succesfully decoded as SpeedCtrl
      // Now, Check whether it was Command.
      const TCommand *cmd = d.GetCommand();
      if (cmd) {
        switch(cmd->cmd) {
        case KReplyGetCommandIntervals:{
         
          Lock();
          iMaxInterval = d.GetCommandIntervals()->maxInterval;
          iMinInterval = d.GetCommandIntervals()->minInterval;
         
          iIntervalsSet = true;
          Notify(KReplyIntervals);
          Unlock();

          break;
        }
        case KReplyGetSpeedLimits:{

          Lock();
          iMaxSpeeds = *(d.GetMaxSpeedLimit());
          iMinSpeeds = *(d.GetMinSpeedLimit());
          iSpeedLimitsSet = true;
          Notify(KReplySpeeds);
          Unlock();
          break;
        }

        case KEventSpeed:{

          Lock();
          iSpeedData = d;
          ++iSpeedSeq;
          Notify(KEventSpeedData);
          Unlock();

          // Call the handler if set.
          if (iSpeedCtrlClientCallback) {
            dPrint(8,"Got Callback for SpeedCtrlDataEvent - executing!");
            iSpeedCtrlClientCallback->OnSpeedCtrlDataEvent(d,
                                                           iSpeedSeq,
                                                           *this,
                                                           iCallBackUserPtr);
            dPrint(8,"Callback for SpeedCtrlDataEvent - execution complete!");
            
          }


          break;
        }
        default:
          dPrint(4,"Unidentified command! %d (%x)! Ignore.",
                 cmd->cmd, cmd->cmd);
          break;
        }
      }
    }else{
      dPrint(5,"DecodeFrom failed");
    }
  }else{
    dPrint(5,"Decode failed");
  }


  return result;


}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
